#include <Algorithm/threeD_elementary_geometry_tool_library.h>


//@ return array which have four length


double*get_plane_equation_of_triangular_plane(double* tr1,double* tr2,double*tr3)
{
    double a[3]={0},b[3]={0};
    a[0]=tr2[0]-tr1[0];
    a[1]=tr2[1]-tr1[1];
    a[2]=tr2[2]-tr1[2];

    b[0]=tr3[0]-tr2[0];
    b[1]=tr3[1]-tr2[1];
    b[2]=tr3[2]-tr2[2];
    double * n=out_product(a,b);
    if(!normalize(n,3))
    {
        free(n);
        return NULL;
    }
    double * re=(double*)malloc(sizeof(double)*4);
    re[3]=-(n[0]*tr1[0]+n[1]*tr1[1]+n[2]*tr1[2]);
    re[0]=n[0];re[1]=n[1];re[2]=n[2];
    // printf("test :%lf\n",re[0]*tr1[0]+re[1]*tr1[1]+re[2]*tr1[2] );
    // printf("test :%lf\n",re[0]*tr2[0]+re[1]*tr2[1]+re[2]*tr2[2] ); 
    // printf("test :%lf\n",re[0]*tr2[0]+re[1]*tr2[1]+re[2]*tr2[2] ); 
    
    free(n); 
    return re; 
}

//@ p1 is a plane equation
//@ p2 is a plane equation
//return t* n+ p:: line equation
double* get_line_equation_from_2planes(double * p1,double *p2)
{
    double * n=out_product(p1,p2);
    // printf("get line :%lf %lf %lf\n",fabs(n[0]),fabs(n[1]),fabs(n[2]));
    // printf("p1 :%lf %lf %lf\n",p1[0],p1[1],p1[2]);
    // printf("p2 :%lf %lf %lf\n",p2[0],p2[1],p2[2]);

    // if(fabs(n[0])<1e-5&&fabs(n[1])<1e-5&&fabs(n[2])<1e-5)
    // {
    //     printf("is paralle\n");
    //     free(n);
    //     return NULL;
    // }
    if(!normalize(n,3))
    {
        free(n);
        return NULL;
    } 
    LB_Matrix * m=(LB_Matrix*)malloc(sizeof(LB_Matrix));
    lb_matrix_init_double(m,3,3); 
    double* data=(double*)(m->data);
    data[0*3+0]=p1[0];data[0*3+1]=p1[1];data[0*3+2]=p1[2];

    data[1*3+0]=p2[0];data[1*3+1]=p2[1];data[1*3+2]=p2[2];

    data[2*3+0]=n[0];data[2*3+1]=n[1];data[2*3+2]=n[2];

    LB_Matrix*mi=m->inverse(m);
    if(mi==NULL)
    {
        free(n);
        lb_matrix_free(m);
        return NULL;
    } 
    double *data1=(double*)(mi->data);
    double b[3]={-p1[3],-p2[3],0};
    double * re=(double*)malloc(sizeof(double)*6);
    for(int i=0;i<3;i++)
    {
        re[i+3]=0;
        for(int j=0;j<3;j++)
        {
            // double temp_d=0;
            // for(int k=0;k<4;k++)
            // {
            //     temp_d+=data[i*4+k]*data1[k*4+j];
            // }
            re[i+3]+=b[j]*data1[i*3+j];
        }
    } 
    re[0]=n[0];re[1]=n[1];re[2]=n[2];
    lb_matrix_free(mi);
    lb_matrix_free(m);
    free(n);
    return re; 
}



// static inline int is_line_same_dir_with_line_segment(double* l,double* v1 ,double* v2)
// {
//   //  is same dir
//     double l1[3]={v2[0]-v1[0],v2[1]-v1[1],v2[2]-v1[2]};
  

//     printf("l:%lf %lf %lf %lf %lf %lf\n",l[0],l[1],l[2],l[3] );
//     printf("point dis line :%lf\n",distance_point_to_line(v1,l) );
//     normalize(l1);

//       printf("l1:%lf %lf %lf\n",l1[0],l1[1],l1[2] );
//     double * n3=out_product(l,l1);
//     if(!normalize(n3))
//     {
//        // printf("is sanem dir\n");
//         SAFE_FREE(n3);
//         return 1; 
//     } 
//     SAFE_FREE(n3);
//    return 0; 
// }

//is line intersecting with line segment


int is_line_intersecting_with_line_segment(double* l,double* v1,double *v2)
{

    double d1[3]={v1[0]-l[3],v1[1]-l[4],v1[2]-l[5]};
    double d2[3]={v2[0]-l[3],v2[1]-l[4],v2[2]-l[5]};
 
    double * n1=out_product(l,d1);
    double* n2=out_product(l,d2);
    normalize(n1,3);normalize(n2,3);
    
   
    if(inner_product(n1,n2,3)<=1e-10)
    {
        free(n1);free(n2);
        return 1;
    }
    free(n1);free(n2);
    
    return 0; 
}


int is_line_segment_intersecting_with_line_segment(double * v1,double* v2,double *vv1,double*vv2)
{
    int re=0;
    double p1[3]={vv1[0]-v1[0],vv1[1]-v1[1],vv1[2]-v1[2]};
    double p2[3]={vv1[0]-v2[0],vv1[1]-v2[1],vv1[2]-v2[2]};
    double p3[3]={vv2[0]-v1[0],vv2[1]-v1[1],vv2[2]-v1[2]};
    double p4[3]={vv2[0]-v2[0],vv2[1]-v2[1],vv2[2]-v2[2]};
    double* dir1=out_product(p1,p2);
    double* dir2=out_product(p3,p4);
    int mark1=normalize(dir1,3),mark2=normalize(dir2,3);
    if(mark1==0)
    {

        free(dir1);free(dir2);
        if(inner_product(p1,p2,3)<=0)
        {
            return 1;
        }
        else if(mark2==0&&inner_product(p3,p4,3)<=0){
            return 1;
        }
        else
        {
            return 0;
        }
    }
    else if(mark2==0)
    {
        free(dir1);free(dir2);
        if(inner_product(p3,p4,3)<=0)
        {
            return 1;
        } 
        else {return 0;}
    }
    double temp=inner_product(dir1,dir2,3);
    free(dir1);free(dir2);
    if(mark1==0||mark2==0||1-fabs(temp)<1e-8)
    {
        //共面但不共线
        dir1=out_product(p1,p3);dir2=out_product(p2,p4);
        //normalize(dir1,3);normalize(dir2,3);
        double temp1=inner_product(dir1,dir2,3);
        free(dir1);free(dir2); 
        if(temp<=0&&temp1<=0)
        {
            return 1;
        }
        else{

            return 0;
        }
    } 
    //free(dir1);free(dir2);
    return 0;
}


//@l :: t*n+p
//@poly::polygon
// @return t1
static double * line_intersection_with_polygon(double* l,double** poly ,int len)
{
    double* re=(double*)malloc(sizeof(double)*2);
    int temp_i=0,mark=-1;
    for(int i=0;i<len&&temp_i<2;i++)
    { 
       // printf("dis:%lf\n",distance_point_to_line(poly[i],l) );
        if(distance_from_point_to_line(poly[i],l)<1e-7)
        {
            mark=i;
            re[temp_i]=(poly[i][0]-l[3])/l[0];
            temp_i++; 
        }
    }
    for(int i=0;i<len&&temp_i<2;i++)
    {
        int j=(i+1)%len;
        if(i==mark||j==mark)
        {
            continue;
        } 
        if(!is_line_intersecting_with_line_segment(l,poly[i],poly[j]))
        {
            continue;
        }
        double* l1=get_line_equation_from_two_points(poly[i],poly[j]);
        if(l1==NULL)
        {continue;} 
        double t= line_point_nearest_2_line(l,l1);
        SAFE_FREE(l1); 
        re[temp_i]=t;
        temp_i++;
        if(temp_i==2&&fabs(re[0]-t)<1e-8)
        {
            temp_i--;
        } 
    }
    if(temp_i==0)
    {
        free(re);
        return NULL;
    }
    else if(temp_i==1)
    {
        re[1]=re[0];
    }
    if(re[1]<re[0])
    {
        double tempd=re[0];
        re[0]=re[1];
        re[1]=tempd;
    }
    return re; 
}

// static inline double * get_intersection_points_from_two_line_segments(double* l11,double* l12,double* l21,double* l22 )
// {
//     double *l1=get_line_equation_from_two_p(l11,l12);
//     double* l2=get_line_equation_from_two_p(l21,l22);
//     double* re=NULL;
//     if(l1==NULL||l2==NULL)
//     {
//         SAFE_FREE(l1);SAFE_FREE(l2);
//         return NULL;
//     }

//     if(is_line_intersecting_with_line_segment(l1,l21,l22)&&is_line_intersecting_with_line_segment(l2,l11,l12))
//     {
//         double t=line_point_nearest_2_line(l1,l2);
//         re=(double*)malloc(sizeof(double)*3);
//         re[0]=t*l1[0]+l1[3];re[1]=t*l1[1]+l1[4];re[2]=t*l1[2]+l1[5];
//         return re;
//     } 

//     SAFE_FREE(l1);SAFE_FREE(l2);
//     return NULL;
// }
//在同一个平面上的两个多边形相交点
// static inline double** get_intersection_points_from_two_polygons_on_one_plane(double **poly1,int len1,double** poly2,int len2)
// {
//     double ** re=(double**)malloc(sizeof(double*)*2);
//     re[0]=NULL;re[1]=NULL;
//     int temp_i=0;
//     for(int i=0;i<len1;i++)
//     {
//         int i1=i,i2=(i+1)%len1;
//         for(int j=0;j<len2;j++)
//         {
//             int j1=j,j2=(j+1)%len2;
//             double *vv=get_intersection_points_from_two_line_segments(poly1[i1],poly1[i2],poly2[j1],poly2[j2]);

//             if(vv!=NULL&&temp_i<2)
//             {
//                 re[temp_i]=vv;
//                 temp_i++;
//             } 
//         }
//     }
//     return re;
// }

//@return double [2][3];
// poly1 poly2 not in one plane(不在同一个平面上)
//保证poly1和poly2是凸的
//if no intersections return NULL
double ** get_intersection_points_from_two_polygons(double**poly1,int len1,double**poly2 ,int len2)
{
    double * p1=get_plane_equation_of_triangular_plane(poly1[0],poly1[1],poly1[2]); 
    double * p2=get_plane_equation_of_triangular_plane(poly2[0],poly2[1],poly2[2]);
    if(p1==NULL||p2==NULL)
    {
        SAFE_FREE(p1);SAFE_FREE(p2);   
        return NULL;
    } 
    double *l =get_line_equation_from_2planes(p1,p2);
    if(l==NULL)
    {
        free(p1);free(p2);
        return NULL;
    }
    double *t1=line_intersection_with_polygon(l,poly1,len1);
    if(t1==NULL)
    {
        free(p1);free(p2);free(l);
        return NULL;
    }
    
    double * t2=line_intersection_with_polygon(l,poly2,len2); 
    if(t2==NULL||t2[0]>t1[1]||t2[1]<t1[0])
    {
        SAFE_FREE(t2);free(t1);free(p1);free(p2);free(l);
        return NULL;
    }
    double ** re=(double**)malloc(sizeof(double*)*2);
    double t3[2];
    for(int i=0;i<2;i++)
    {
        t2[0]>t1[0]?(t3[0]=t2[0]):(t3[0]=t1[0]);
        t2[1]<t1[1]?(t3[1]=t2[1]):(t3[1]=t1[1]); 
    }
    for(int i=0;i<2;i++)
    {
        re[i]=(double*)malloc(sizeof(double)*3);
        for(int j=0;j<3;j++)
        {
            re[i][j]=t3[i]*l[j]+l[3+j];
        }
    } 
    free(t1);free(t2);free(p1);free(p2);free(l);
    return re;

}


static double * get_coordinate_in_another_coordinate_system(double* p,double* a1,double *a2,double*a3)
{
    LB_Matrix* m=(LB_Matrix*)malloc(sizeof(LB_Matrix));
    lb_matrix_init_double(m,3,3);
    double* data=(double*)(m->data);
    data[0*3+0]=a1[0]; data[1*3+0]=a1[1];  data[2*3+0]=a1[2];   
    data[0*3+1]=a2[0]; data[1*3+1]=a2[1];  data[2*3+1]=a2[2];
    data[0*3+2]=a3[0]; data[1*3+2]=a3[1];  data[2*3+2]=a3[2];
    LB_Matrix* mi=m->inverse(m);
    if(mi==NULL)
    {
        lb_matrix_free(m);
        return NULL;
    } 
    data=(double*)(mi->data);
    double * re=(double*)malloc(sizeof(double)*3); 
    memset(re,0,sizeof(double)*3);
    for(int i=0;i<3;i++)
    {
        re[i]=0;
        for(int j=0;j<3;j++)
        {
            re[i]+=data[i*3+j]*p[j];
        } 
    }
    lb_matrix_free(mi);
    lb_matrix_free(m);
    return re;
}


//return the triangle coordinate of p in p1 p2 p3
double* get_triangle_coordinate(double* p,double * p1,double*p2,double* p3 )
{
    double a1[3]={p1[0]-p3[0],p1[1]-p3[1],p1[2]-p3[2]};
    double a2[3]={p2[0]-p3[0],p2[1]-p3[1],p2[2]-p3[2]};
    double pp[3]={p[0]-p3[0],p[1]-p3[1],p[2]-p3[2]};
    double* n=out_product(a1,a2);
    double* coord=get_coordinate_in_another_coordinate_system(pp,a1,a2,n);
    free(n);
    if(coord==NULL)
    {
        return NULL;
    }
    double *re=(double*)malloc(sizeof(double)*3);
    re[0]=coord[0];re[1]=coord[1];
    re[2]=1-re[0]-re[1]; 
    free(coord);
    return re;
}
double compute_area_of_triangle(double* tri1,double* tri2,double* tri3)
{
    double a=distance_of_two_points(tri1,tri2,3), b=distance_of_two_points(tri2,tri3,3),
        c=distance_of_two_points(tri3,tri1,3);
    double p=(a+b+c)/2.0; 

    return SAFE_SQRT(p*(p-a)*(p-b)*(p-c));
}
